/**
*
* @file     GUI_Layer.c
* @author   IMS Systems Lab - ART Team
* @version  V1.5
* @date     09/06/2010
* @brief    This file provides all the API between the Application and PC serial communication.
* @details
*
* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
* TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
* DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
* FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
* CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
*
* THIS SOURCE CODE IS PROTECTED BY A LICENSE.
* FOR MORE INFORMATION PLEASE CAREFULLY READ THE LICENSE AGREEMENT FILE LOCATED
* IN THE ROOT DIRECTORY OF THIS FIRMWARE PACKAGE.
*
* <h2><center>&copy; COPYRIGHT 2009 STMicroelectronics</center></h2>
*
*/


/**
 * \include
*/

#include "GUI_Layer.h"
#include "string.h"
#include "utils.h"

/**
 * \defgroup GUI_Layer
 * \{
 */

/**
 * @brief Specifies the incremental counter. It should be used from a client to
 */
static u16 s_iC = 0;

/**
 * @brief Number of sample to be acquired
 */
static u16 s_nLengthSample = 0;



/**
*  @brief specifies which output data are enabled
*  @details: 0 Disable/1 Enable order bit :AHRS | data Mode*(2 bit) | Accelration | Gyro | Magnetometer | Pressure | temperature
*     * data MOde 00-> non calibrated data * sensitivity [mg, dps etc..] 
*                 01-> non calibrated data [lsb]
*                 10-> calibrated data (Sesnitivity and offset computed by user calibration)
*/
static u8 s_uOutSelect = 0x1F;


/**
*  @brief specifies comunicatin frequency and output channel 
*  @details :RFU 0 | RFU 0 | FQ2 | FQ1 | FQ0 | OT2 | OT1 | OT0
*      FQ : 000 (1Hz); 001 (10Hz); 010 (25Hz); 011 (50Hz) 
*      OT : 000 (USB output) 
*/

static u8 s_uOutMode=0;

/**
*  @brief specifies if the iNEMO is connected
*/

static bool s_bConnectState=FALSE;

/**
 *  @brief sensor sampling frequency
 */

static u8 s_uTimerFrequence=0;


/**
 * @brief Specifies if the sensor data have to be filtered using AHRS.
 */
static bool s_bAhrsEnabled = FALSE;

/**
 * @brief Specifies if the raw sensor data sending is enabled.
 */
static bool s_bRawDataEnabled = FALSE;


/**
 * @brief Specifies if the user calibration sensor data sending is enabled.
 */
static bool s_bCalDataEnabled = FALSE;


/**
* @addtogroup GUI_Layer_API
* @{
*/
//static void Enable_Timer(FunctionalState command);


/**
 * \brief Process a command coming from the PC GUI. All supported command are: <ul>
 * <li>iNEMO_Connect - this must be the first command to send to the board. Others command are
 * not processed before this one.</li>
 * <li>iNEMO_Disconnect </li>
 * <li>iNEMO_Reset_Board </li>
 * <li>iNEMO_Enter_DFU_Mode </li>
 * <li>iNEMO_Led </li>
 * <li>iNEMO_Get_MCU_ID </li>
 * <li>iNEMO_Get_FW_Version </li>
 * <li>iNEMO_Get_HW_Version </li>
 * <li>iNEMO_Get_AHRS_Library </li>
 * <li>iNEMO_Identify </li>
 * <li>iNEMO_SetOutMode </li>
 * <li>iNEMO_GetOutMode </li> 
 * <li>iNEMO_Start_Acquisition - start the the transmission of sensor data according to the
 * parameters specified in frame payload.</li>
 * <li>iNEMO_Stop_Acquisition - stop the transmission of sensor data.</li>
 * </ul>
 * Invalid frame are  dropped.
 *
 * @param nFrameSize specifies the size of the received frame.
 */
void ParseGuiFrame(u32 nFrameSize) {
 
 /** Check if the iNEMO is connect, if it is disconnected no the only accepted command is iNEMO_Connect */  
  if(s_bConnectState == FALSE )
          {
             /* if the command if iNEMO_Connect Parse the frame */ 
            if(buffer_rx[2]== iNEMO_Connect)
            {
              if(buffer_rx[0]==CTRL_wACK_LF && buffer_rx[1]==1 && nFrameSize== buffer_rx[1]+2)
              {  
                /* iNEMO_Connet_Response --> Change iNEMO Stauts to Connceted and Send Ack*/
                s_bConnectState=TRUE;
                Send_Ack(iNEMO_Connect,1,0x00);
              }
              else
              {
                /* wrong  syntax */
                Send_Nack(iNEMO_Connect,WrongSyntax);
              }
            }
            else  /* the command is not iNEMO_Connect */
            {
              Send_Nack((char)buffer_rx[2], iNEMONotConneted);
            }
          }
  else  /* if iNEMO is connected the other command can be execited   */
      {
   /* Switch the command       */
     switch (buffer_rx[2]){
          
      /* Connection Request*/  
        case iNEMO_Connect:  
          /* syntactic check :
            Frame Control -> Control frame; Ach required; Last fragment ; version 1; Normal Priority 
            Lenght -> 1 (No payload)
            nFrameSize --> check il the payload lenght is correct          
          */
          if(buffer_rx[0]==CTRL_wACK_LF && buffer_rx[1]==1  && nFrameSize== buffer_rx[1]+2)
          {  
            /* iNEMO_Connet_Response --> Change iNEMO Stauts to Connceted and Send Ack*/
            s_bConnectState=TRUE;
            Send_Ack(iNEMO_Connect,1,0x00);
          }
          else
          {
            Send_Nack(iNEMO_Connect,WrongSyntax);
          }
          break;
          
     /*  Disconnect Request*/   
        case iNEMO_Disconnect:
          /* syntactic check :
            Frame Control -> Control frame; Ach required; Last fragment ; version 1; Normal Priority 
            Lenght -> 1 (No payload)
            nFrameSize --> check il the payload lenght is correct          
          */          
          if(buffer_rx[0]==CTRL_wACK_LF && buffer_rx[1]==1  && nFrameSize== buffer_rx[1]+2)
          {
            /* iNEMO_Connet_Response --> Change iNEMO Stauts to Disconnceted and Send Ack */
            s_bConnectState=FALSE;
            Send_Ack(iNEMO_Disconnect,1,0x00);
          }
          else
           Send_Nack(iNEMO_Connect,WrongSyntax);
          break;
          
     /* Reset Request*/   
        case iNEMO_Reset_Board:
          /* syntactic check :
            Frame Control -> Control frame; Ack NOTrequired; Last fragment ; version 1; Normal Priority 
            Lenght -> 1 (No payload)
            nFrameSize --> check il the payload lenght is correct          
          */          
          if(buffer_rx[0]==CTRL_wACK_LF && buffer_rx[1]==1  && nFrameSize== buffer_rx[1]+2)
          {
            Send_Ack(iNEMO_Reset_Board,1,0x00);
            vTaskDelay(5000 / portTICK_RATE_MS); /*delay to allow the GUI receive ACK*/
            s_bConnectState=FALSE;
            Reset_Device();
          }
          else
           Send_Nack(iNEMO_Reset_Board,WrongSyntax);
          break;          

     /* iNEMO_Enter_DFU_Mode Request*/   
        case iNEMO_Enter_DFU_Mode:
          /* syntactic check :
            Frame Control -> Control frame; Ack required; Last fragment ; version 1; Normal Priority 
            Lenght -> 1 (No payload)
            nFrameSize --> check il the payload lenght is correct          
          */          
          if(buffer_rx[0]==CTRL_wACK_LF && buffer_rx[1]==1  && nFrameSize== buffer_rx[1]+2 )
          {
            Send_Ack(iNEMO_Enter_DFU_Mode,1,0x00);
            #ifdef Enable_SW_Mode
            Enter_DFU_SW_Mode();
            #endif
            Reset_Device();
          }
          else
           Send_Nack(iNEMO_Enter_DFU_Mode,WrongSyntax);
          break; 
          
          
        case iNEMO_Led:  
          /* syntactic check :
            Frame Control -> Control frame; Ach required; Last fragment ; version 1; Normal Priority 
            Lenght -> 1 (No payload)
            nFrameSize --> check il the payload lenght is correct          
          */
          if(buffer_rx[0]==CTRL_wACK_LF && buffer_rx[1]==2  && nFrameSize== buffer_rx[1]+2)
          { 
            if(buffer_rx[3])
              iNEMO_Led_On(LED1);
            else
              iNEMO_Led_Off(LED1);
            /* iNEMO_Connet_Response --> Change iNEMO Stauts to Connceted and Send Ack*/
            Send_Ack(iNEMO_Led,1,0x00);
          }
          else
          {
            Send_Nack(iNEMO_Connect,WrongSyntax);
          }
          break;          
          
     /*  iNEMO_Get_MCU_ID  Request :  */   
        case iNEMO_Get_MCU_ID:
          /* syntactic check :
            Frame Control -> Control frame; Ack required; Last fragment ; version 1; Normal Priority 
            Lenght -> 1 
            nFrameSize --> check il the payload lenght is correct          
          */          
          if(buffer_rx[0]==CTRL_wACK_LF && buffer_rx[1]==1  && nFrameSize== buffer_rx[1]+2 )  
          {
            unsigned char mcu_id[12];
            for (int i=0; i<12; ++i)
            mcu_id[i] = MCU_ID[12-(i+1)];                        
            /* send ack with payload= STM32 Unique indetifier */            
            Send_Ack(iNEMO_Get_MCU_ID, 13, mcu_id);
          }
          else
           Send_Nack(iNEMO_Get_MCU_ID,WrongSyntax);
          break;  
          
     /*  iNEMO Get FW Version Request :  */   
        case iNEMO_Get_FW_Version:
          /* syntactic check :
            Frame Control -> Control frame; Ack required; Last fragment ; version 1; Normal Priority 
            Lenght -> 1 
            nFrameSize --> check il the payload lenght is correct          
          */          
          if(buffer_rx[0]==CTRL_wACK_LF && buffer_rx[1]==1  && nFrameSize== buffer_rx[1]+2 )  
          {
            /* send ack with payload= string FW version*/            
            Send_Ack(iNEMO_Get_FW_Version, (SIZE_FWversion + 1), iNEMO_FIRMWARE_VERSION);
          }
          else
           Send_Nack(iNEMO_Get_FW_Version,WrongSyntax);
          break;                       
                   
     /*  iNEMO Get HW Version Request :  */   
        case iNEMO_Get_HW_Version:
          /* syntactic check :
            Frame Control -> Control frame; Ack required; Last fragment ; version 1; Normal Priority 
            Lenght -> 1 
            nFrameSize --> check il the payload lenght is correct          
          */          
          if(buffer_rx[0]==CTRL_wACK_LF && buffer_rx[1]==1  && nFrameSize== buffer_rx[1]+2 )  
          {
            /* send ack with payload= string HW version*/            
            Send_Ack(iNEMO_Get_HW_Version, (SIZE_HWversion + 1), iNEMO_HARDWARE_VERSION);
          }
          else
           Send_Nack(iNEMO_Get_HW_Version,WrongSyntax);
          break;      
          
     /*  iNEMO iNEMO_Get_AHRS_Library : Enabled or Disable  */   
        case iNEMO_Get_AHRS_Library:
          /* syntactic check :
            Frame Control -> Control frame; Ack required; Last fragment ; version 1; Normal Priority 
            Lenght -> 1 
            nFrameSize --> check il the payload lenght is correct          
          */          
          if(buffer_rx[0]==CTRL_wACK_LF && buffer_rx[1]==1  && nFrameSize== buffer_rx[1]+2 )  
          {
            /* send ack with payload= string HW version*/            
            Send_Ack(iNEMO_Get_AHRS_Library, (SIZE_AHRSlibrary + 1), iNEMO_AHRS_LIBRARY);
          }
          else
           Send_Nack(iNEMO_Get_AHRS_Library,WrongSyntax);
          break;           
          
     /*  iNEMO_Identify Request :  */   
        case iNEMO_Identify:
          /* syntactic check :
            Frame Control -> Control frame; Ack required; Last fragment ; version 1; Normal Priority 
            Lenght -> 1 
            nFrameSize --> check il the payload lenght is correct          
          */          
          if(buffer_rx[0]==CTRL_wACK_LF && buffer_rx[1]==1  && nFrameSize== buffer_rx[1]+2 )  
          {
            iNEMO_Led_On(LED1); 
            unsigned char mcu_id[12];
            for (int i=0; i<12; ++i)
            mcu_id[i] = MCU_ID[12-(i+1)];                        
            /* send ack with payload= STM32 Unique indetifier */            
            Send_Ack(iNEMO_Get_MCU_ID, 13, mcu_id);
            for (int i=0; i<7; ++i) {
            iNEMO_Led_Toggle(LED1);
            vTaskDelay(500 / portTICK_RATE_MS);
            }
          }
          else
           Send_Nack(iNEMO_Identify,WrongSyntax);
          break; 
          
          
/*-------Acquisition Command--------------- */          
          
        /* Configure iNEMO output settings*/
        case iNEMO_SetOutMode:
          /* syntactic check :
            Frame Control -> Control frame; Ack required; Last fragment ; version 1; Normal Priority 
            Lenght -> 5 
            nFrameSize --> check il the payload lenght is correct
            buffer_rx[3]  --> Enable setting 1 Enable/0 Disable AHRS |calibrate | Raw | Acc | Gyro | Mag | temp |
            buffer_rx[4]  --> Acquition Frequency + output type : 00 FQ2 FQ1 FQ0 OT2 OT1 OT0 
            buffer_rx[5]  --> Number of Samples (MSB)
            buffer_rx[6]  --> Number of Samples (LSB)
          */          
          if(buffer_rx[0]==CTRL_wACK_LF && buffer_rx[1]==5  && nFrameSize== buffer_rx[1]+2 && ((buffer_rx[4] & 0xC0)==0x00)) 
          {           
#ifndef iNEMO_AHRS
            if(buffer_rx[3] & 0x80)
              Send_Nack(iNEMO_SetOutMode,NotExecutable);
            else
            {
            SetAhrs(buffer_rx[3] & 0x80 ? TRUE : FALSE);
            SetRawData(buffer_rx[3] & 0x20 ? TRUE : FALSE);
            SetCalData(buffer_rx[3] & 0x40 ? TRUE : FALSE);
            
            if ((GetCalData() & GetRawData()))
            {
              Send_Nack(iNEMO_SetOutMode,NotExecutable);
            }
            else
            {  
            s_uOutSelect=buffer_rx[3];   
            /* set timer */
            if(GetAhrs())
            s_uTimerFrequence = 0x03;
            else
            s_uTimerFrequence = (buffer_rx[4] & 0x38) >> 3;
            //Set_Timer(s_uTimerFrequence);
            /* set number of sample or continuos mode (s_nLengthSample=0) */
            s_nLengthSample = ((u16)buffer_rx[5] << 8) + buffer_rx[6];
           /*Select output destination*/
            s_uOutMode =(buffer_rx[4] & 0x03);   // USB mode 
            Send_Ack(iNEMO_SetOutMode, 1, 0x00);          
          }
            }
#else
            SetAhrs(buffer_rx[3] & 0x80 ? TRUE : FALSE);
            SetRawData(buffer_rx[3] & 0x20 ? TRUE : FALSE);
            SetCalData(buffer_rx[3] & 0x40 ? TRUE : FALSE);
            
            if ((GetCalData() & GetRawData()))
            {
              Send_Nack(iNEMO_SetOutMode,NotExecutable);
            }
            else
            {  
            s_uOutSelect=buffer_rx[3];   
            /* set timer */
            if(GetAhrs())
            s_uTimerFrequence = 0x03;
            else
            s_uTimerFrequence = (buffer_rx[4] & 0x38) >> 3;
            //Set_Timer(s_uTimerFrequence);
            /* set number of sample or continuos mode (s_nLengthSample=0) */
            s_nLengthSample = ((u16)buffer_rx[5] << 8) + buffer_rx[6];
           /*Select output destination*/
            s_uOutMode =(buffer_rx[4] & 0x03);   // USB mode 
            Send_Ack(iNEMO_SetOutMode, 1, 0x00);          
          }                   
            
#endif          
          }
          else
           Send_Nack(iNEMO_SetOutMode,WrongSyntax);
          break;  
          
        /* Get iNEMO output settings*/
        case iNEMO_GetOutMode:
          /* syntactic check :
            Frame Control -> Control frame; Ack required; Last fragment ; version 1; Normal Priority 
            Lenght -> 1
            nFrameSize --> check il the payload lenght is correct
          Ack response
            buffer_rx[3]  --> Enable setting 1 Enable/0 Disable AHRS |calibrate | Raw | Acc | Gyro | Mag | temp |
            buffer_rx[4]  --> Acquition Frequency + output type : 00 FQ2 FQ1 FQ0 OT2 OT1 OT0 
            buffer_rx[5]  --> Number of Samples (MSB)
            buffer_rx[6]  --> Number of Samples (LSB)
          */          
          if(buffer_rx[0]==CTRL_wACK_LF && buffer_rx[1]==1  && nFrameSize== buffer_rx[1]+2 ) 
          {
            u8 temp[4];
            temp[0]=s_uOutSelect;
            temp[1]=((s_uTimerFrequence ) << 3) + (s_uOutMode & 0x07);
            temp[2]=(u8)(s_nLengthSample >> 8);
            temp[3]=(u8)s_nLengthSample;    
            Send_Ack(iNEMO_GetOutMode, 5, temp);
          }
          else
           Send_Nack(iNEMO_GetOutMode,WrongSyntax);
          break;          
          
     /* start iNEMO acquisition data*/   
     case iNEMO_Start_Acquisition:
          /* syntactic check :
            Frame Control -> Control frame; Ack required; Last fragment ; version 1; Normal Priority 
            Lenght -> 1
            nFrameSize --> check il the payload lenght is correct
          */          
          if(buffer_rx[0]==CTRL_wACK_LF && buffer_rx[1]==1  && nFrameSize== buffer_rx[1]+2) 
          {
            s_iC=0;
            //Enable_Timer(ENABLE);
            Send_Ack(iNEMO_Start_Acquisition, 1, 0x00);
          }
          else
           Send_Nack(iNEMO_SetOutMode,WrongSyntax);
          break;  

     /* start iNEMO acquisition data*/   
     case iNEMO_Stop_Acquisition:
          /* syntactic check :
            Frame Control -> Control frame; Ack required; Last fragment ; version 1; Normal Priority 
            Lenght -> 1
            nFrameSize --> check il the payload lenght is correct
          */          
          if(buffer_rx[0]==CTRL_wACK_LF && buffer_rx[1]==1  && nFrameSize== buffer_rx[1]+2) 
          {
            //Enable_Timer(DISABLE);
            //TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
            s_iC=0;
            Send_Ack(iNEMO_Stop_Acquisition, 1, 0x00);
          }
          else
           Send_Nack(iNEMO_SetOutMode,WrongSyntax);
          break;  
          
        default:
           Send_Nack(buffer_rx[2],CmdUnsupported);
          break;
        }
    
  }
  
}

/**
 * \brief Send a frame containing all sensor data and orientation data according to the transmission option specified by the CMD_START command.
 * \param sensorData : pointer to the iNEMO_SENSORDATA struct to send
 * \param angle : pointer to the iNEMO_EULER_ANGLES struct to send
 * \param quat : pointer to the iNEMO_QUAT struct to send
 * \retval NONE
 */
void DataProcess(u8 outmode, iNEMO_DATA *pData){
  u8 nByteToSend=0;  
  u8 databuffer[60]= {0};
 
  if (s_iC < s_nLengthSample | s_nLengthSample ==0)
  {

      /* if accelerometer output is requested */
      if(outmode & 0x10) 
      {
      LSM303DLH_Acc_Read_RawData(pData->sAcc);        
        /*if raw data enabled*/
        if(s_bRawDataEnabled)
        {
          s16_to_u8_buffer(&(pData->sAcc[0]), &databuffer[nByteToSend + 2]);
          s16_to_u8_buffer(&(pData->sAcc[1]), &databuffer[nByteToSend + 4]);
          s16_to_u8_buffer(&(pData->sAcc[2]), &databuffer[nByteToSend + 6]);
        }
        else
        {
          /* if user calibration enable*/
          if(GetCalData())
          {
            s16 acc[3];
            acc[0]=(s16)(pData->sAcc[0]/pData->uGain[0]) - pData->sOffset[0];
            acc[1]=(s16)(pData->sAcc[1]/pData->uGain[1]) - pData->sOffset[1];
            acc[2]=(s16)(pData->sAcc[2]/pData->uGain[2]) - pData->sOffset[2];                
            s16_to_u8_buffer(&acc[0], &databuffer[nByteToSend + 2]);
            s16_to_u8_buffer(&acc[1], &databuffer[nByteToSend + 4]);
            s16_to_u8_buffer(&acc[2], &databuffer[nByteToSend + 6]);
          }
          /* else send calibrated data using default setting*/
          else
          {
            s16 acc[3];
            acc[0]=(pData->sAcc[0]/iNEMO_Default_GainAccX);
            acc[1]=(pData->sAcc[1]/iNEMO_Default_GainAccY);
            acc[2]=(pData->sAcc[2]/iNEMO_Default_GainAccZ);                
            s16_to_u8_buffer(&acc[0], &databuffer[nByteToSend + 2]);
            s16_to_u8_buffer(&acc[1], &databuffer[nByteToSend + 4]);
            s16_to_u8_buffer(&acc[2], &databuffer[nByteToSend + 6]);    
          }
        }
        nByteToSend+=6;        
        
      }
      /* if gyroscopes output is requested */      
      if(outmode & 0x08)
      {
        LPRYxxxAL_Read_RawRate(pData->sGyro);         
        /*if raw data enabled*/        
        if(s_bRawDataEnabled)
        {
          s16_to_u8_buffer(&(pData->sGyro[0]), &databuffer[nByteToSend + 2]);
          s16_to_u8_buffer(&(pData->sGyro[1]), &databuffer[nByteToSend + 4]);
          s16_to_u8_buffer(&(pData->sGyro[2]), &databuffer[nByteToSend + 6]);
        }
        else
        {
          /*if user calibrated data enabled*/          
          if(GetCalData())
          {
            s16 dps[3];
            dps[0]=(s16)(pData->sGyro[0]/pData->uGain[3]) - pData->sOffset[3];
            dps[1]=(s16)(pData->sGyro[1]/pData->uGain[4]) - pData->sOffset[4];
            dps[2]=(s16)(pData->sGyro[2]/pData->uGain[5]) - pData->sOffset[5];                
            s16_to_u8_buffer(&dps[0], &databuffer[nByteToSend + 2]);
            s16_to_u8_buffer(&dps[1], &databuffer[nByteToSend + 4]);
            s16_to_u8_buffer(&dps[2], &databuffer[nByteToSend + 6]);
          }
          else
          {
            /*if else send calibrated data using default setting*/            
            s16 dps[3];
            dps[0]=(s16)(pData->sGyro[0]/iNEMO_Default_GainGyroX);
            dps[1]=(s16)(pData->sGyro[1]/iNEMO_Default_GainGyroY);
            dps[2]=(s16)(pData->sGyro[2]/iNEMO_Default_GainGyroZ);                
            s16_to_u8_buffer(&dps[0], &databuffer[nByteToSend + 2]);
            s16_to_u8_buffer(&dps[1], &databuffer[nByteToSend + 4]);
            s16_to_u8_buffer(&dps[2], &databuffer[nByteToSend + 6]);    
          }
        }
        nByteToSend+=6;             
      }  
      /* if magnetometer output is requested */     
      if(outmode & 0x04)
      {
        LSM303DLH_Magn_Read_RawData(pData->sMag);
        /*if raw data enabled*/        
        if(GetRawData())
        {  
          s16_to_u8_buffer(&(pData->sMag[0]), &databuffer[nByteToSend + 2]);
          s16_to_u8_buffer(&(pData->sMag[1]), &databuffer[nByteToSend + 4]);
          s16_to_u8_buffer(&(pData->sMag[2]), &databuffer[nByteToSend + 6]);
        }
        else
        {
          /*if user calibrated data enabled*/          
          if(GetCalData())
          {
            s16 gauss[3]; 
            gauss[0]=(s16)(pData->sMag[0]*1000/pData->uGain[6]) - pData->sOffset[6];
            gauss[1]=(s16)(pData->sMag[1]*1000/pData->uGain[7]) - pData->sOffset[7];
            gauss[2]=(s16)(pData->sMag[2]*1000/pData->uGain[8]) - pData->sOffset[8];                
            s16_to_u8_buffer(&gauss[0], &databuffer[nByteToSend + 2]);
            s16_to_u8_buffer(&gauss[1], &databuffer[nByteToSend + 4]);
            s16_to_u8_buffer(&gauss[2], &databuffer[nByteToSend + 6]);   
          }
          else
          {
            /*if else send calibrated data using default setting*/              
            s16 gauss[3]; 
            gauss[0]=pData->sMag[0]*1000/iNEMO_Default_GainMagnX;
            gauss[1]=pData->sMag[1]*1000/iNEMO_Default_GainMagnY;
            gauss[2]=pData->sMag[2]*1000/iNEMO_Default_GainMagnZ;                
            s16_to_u8_buffer(&gauss[0], &databuffer[nByteToSend + 2]);
            s16_to_u8_buffer(&gauss[1], &databuffer[nByteToSend + 4]);
            s16_to_u8_buffer(&gauss[2], &databuffer[nByteToSend + 6]);              
          }
        }
        nByteToSend+=6; 
      }
      
      /* if pressure output is requested */       
      if(outmode & 0x02)
      {
        LPS001DL_Get_Raw_Pressure(&(pData->uPress));   
        /*if raw data enabled*/        
        if(GetRawData())
        { 
          u16_to_u8_buffer(&(pData->uPress), &databuffer[nByteToSend + 2]);
        }
        
        /* if user calibrated data enabled*/ 
        else if(s_bCalDataEnabled)
        {
          u16 p;
          p=(s16)(pData->uPress/pData->uGain[9]) - pData->sOffset[9];
          u16_to_u8_buffer(&p, &databuffer[nByteToSend + 2]);             
        }
        else
        {
          /*else send calibrated data using default setting*/    
            u16 p;
            p=(u16)((pData->uPress)/P_Conversion);
            u16_to_u8_buffer(&p, &databuffer[nByteToSend + 2]);             
        }  
        nByteToSend+=2; 
      }
      /* if temperature output is requested */         
      if(outmode & 0x01)
      {
        STLM75_Read_Temperature_Signed(&(pData->sTemp)); 
        s16_to_u8_buffer(&(pData->sTemp), &databuffer[nByteToSend + 2]); 
        nByteToSend+=2;
      }           

      s_iC++;
      databuffer[0]=(u8)(s_iC>>8);
      databuffer[1]=(u8)(s_iC);
      nByteToSend+=2; /* 2 byte counter*/
      
      Send_Data(iNEMO_Start_Acquisition, nByteToSend+1, databuffer);    
  }
  
  else { // end of data acquisition in Sample Mode.
   //Enable_Timer(DISABLE);
  }
}

/**
* \brief  Send Acknowledgment without Payload to PC, as correct reception of a frame.
* \param frame : Frame_Type to acknowledge.
* \retval None
*/
void Send_Ack(unsigned char frame, u8 length, u8* payload)
{
	u8 tmp[30];
	tmp[0] = ACK;
	tmp[1] = length;
        tmp[2] = frame;
        if(length>1)
          CopyBuffer(payload, &tmp[3],length-1);
	Virtual_Com_Write_Buffer(tmp, length + 2);
}


/**
* \brief  Send Command w/o payload to PC, as correct reception of a frame.
* \param frame : Frame_Type to acknowledge.
* \retval None
*/
void Send_Data(unsigned char frame, u8 length, u8* payload)
{
	u8 tmp[100];
	tmp[0] = DATA;
	tmp[1] = length;
        tmp[2] = frame;
        if(length>1)
          CopyBuffer(payload, &tmp[3],length-1);
	Virtual_Com_Write_Buffer(tmp, length + 2);
}

/**
* \brief  Send NAcknowledgment without Payload to PC, as correct reception of a frame.
* \param frame : Frame_Type to acknowledge.
* \retval None
*/
void Send_Nack(unsigned char frame, u8 error_code)
{
        u8 tmp[30];
	tmp[0] = NACK;
	tmp[1] = 2;
        tmp[2] = frame;
        tmp[3] = error_code;
	Virtual_Com_Write_Buffer(&tmp[0], 4);
}


/**
 * \brief Configure the timer in order to generate a recursive interrupt according to the
 * <code>number</code> parameter.
 * The interrupt is used by the iNemo Data task to send the sensor data at a given frequency.
 *
 * @param number one of the following value:<ul>
 * <li>LOW_FREQUENCY - specifies a frequency acquisition of 1Hz</li>
 * <li>MEDIUM_FREQUENCY_1 - specifies a frequency acquisition of 10Hz</li>
 * <li>MEDIUM_FREQUENCY_2 - specifies a frequency acquisition of 25Hz</li>
 * <li>HIGH_FREQUENCY - specifies a frequency acquisition of 50Hz. This is the
 * default value</li>
 * </ul>
 
void Set_Timer(unsigned char number)
{
  unsigned short a;
  unsigned short b;
  unsigned long n;
  int  frequency; //This value is the timer frequency expressed in Hz
  // table to convert the frequency number to actual frequency value.
  const int nFreqEnum2freVal[4] = { 1, 10, 25, 50 };
  TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
  NVIC_InitTypeDef NVIC_InitStructure;

  frequency = number < 4 ? nFreqEnum2freVal[number] : 50;
  TIM_TimeBaseStructInit( &TIM_TimeBaseStructure );

  // Time base configuration for timer 2 - which generates the interrupts.
  n = 72000000/frequency;

  prvFindFactors( n, &a, &b );
  TIM_TimeBaseStructure.TIM_Period = b - 1;
  TIM_TimeBaseStructure.TIM_Prescaler = a - 1;
  TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
  TIM_TimeBaseInit( TIM2, &TIM_TimeBaseStructure );
  TIM_ARRPreloadConfig(TIM2, ENABLE );

  NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 13;
  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
  NVIC_Init( &NVIC_InitStructure );
}
*/
/**
 * \brief Start and stop the timer used by the iNemo Data Task.
 * \param command start the timer if ENABLE, stop the timer if DISABLE.
 */


/**
 * \brief  Set Output Mode option
 * @param outmode : 0 Disable/1 Enable order bit :AHRS | data Mode'(2 bit) | Accelration | Gyro | Magnetometer | Pressure | temperature 
 *     
 *                 ' data MOde 00-> non calibrated data * sensitivity [mg, dps etc..] 
 *                 01-> non calibrated data [lsb]
 *                 10-> calibrated data (Sesnitivity and offset computed by user calibration)
 */
void SetOutMode(u8 outmode) {
  s_uOutSelect = outmode;
}

/**
 * \brief  Return output mode option
 */
u8 GetOutMode() {
  return s_uOutSelect;
}

/**
 * \brief set a boolean to enable/disable  AHRS
 * \param bEnable :  TRUE Ahrs enable FALSE Ahrs disable
 */
void SetAhrs(bool bEnable) {
  s_bAhrsEnabled = bEnable;
}

/**
 * \brief set a boolean to enable/disable  RAW DATA
 * \param bEnable :  TRUE RAW enable FALSE RAW disable
 */
void SetRawData(bool bEnable) {
  s_bRawDataEnabled = bEnable;
}

/**
 * \brief set a boolean to enable/disable User calibration DATA
 * \param bEnable :  TRUE RAW enable FALSE RAW disable
 */
void SetCalData(bool bEnable) {
  s_bCalDataEnabled = bEnable;
}

/**
 * \brief return a boolean to check if AHRS is enabled/disabled
 * \retval s_bAhrsEnabled : TRUE Ahrs enable FALSE Ahrs disable
 */
bool GetAhrs()
{
  return s_bAhrsEnabled;
}

/**
 * \brief return a boolean to check if User cal data is enabled/disabled
 * \retval s_bAhrsEnabled : TRUE Ahrs enable FALSE Ahrs disable
 */
bool GetCalData()
{
  return s_bCalDataEnabled;
}

/**
 * \brief return a boolean to check if Raw is enabled/disabled
 * \retval s_bAhrsEnabled : TRUE Ahrs enable FALSE Ahrs disable
 */
bool GetRawData()
{
  return s_bRawDataEnabled;
}
/**
 * @}
 */

/**
 * @}
 */

/******************* (C) COPYRIGHT 2007 STMicroelectronics *****END OF FILE****/
